using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.DirectX;
using Microsoft.DirectX.Direct3D;

namespace SteeringBehaviors
{
    public partial class TaskForce
    {
        #region HELPER METHODS

        /// <summary>
        /// Either set the focus forward vector and point to that of the 
        /// first ship (leader) or calculate the average forward vector 
        /// and average position to serve as the focus forward vector and 
        /// point respectively.
        /// </summary>
        private void CalculateFocus()
        {
            if (StickToLeader)
            {
                // TODO: focus to leader
				focusForward =  Ships[0].GetForward();
				focusPoint = Ships[0].Position;
            }
            else
            {
				Vector3 newfocusF,newfocusP;

				newfocusF.X = 0;
				newfocusF.Y = 0;
				newfocusF.Z = 0;
				newfocusP.X = 0;
				newfocusP.Y = 0;
				newfocusP.Z = 0;
                // TODO: focus to task force averages
				for (int i = 0; i < Ships.Count; i++)
				{
					newfocusP += Ships[i].Position;
					newfocusF += Ships[i].GetForward();
				}
				newfocusF.Normalize();
				newfocusP.X /= Ships.Count;
				newfocusP.Y /= Ships.Count;
				newfocusP.Z /= Ships.Count;
				//newfocusP.Normalize();
				focusForward = newfocusF;
				focusPoint = newfocusP;
            }
        }

        /// <summary>
        /// Determines the acceleration vector to apply to a ship based
        /// on the alignment algorithm
        /// </summary>
        /// <returns>the acceleration vector to apply</returns>
        private Vector3 GetAlignmentAcceleration()
        {
            Vector3 acceleration = new Vector3();
            if (AlignmentStrength != 0)
            {
                // TODO: Compute acceleration vector based on alignment algorithm.
                // Scale this vector by the AlignmentStrength property.
				acceleration = focusForward;
				acceleration.Scale(AlignmentStrength);
            }
            return acceleration;
        }

        /// <summary>
        /// Determines the acceleration vector to apply to the provided 
        /// ship based on the cohesion algorithm
        /// </summary>
        /// <param name="ship">the provided ship</param>
        /// <returns>the acceleration vector to apply</returns>
        private Vector3 GetCohesionAcceleration(Ship ship)
        {
            Vector3 acceleration = new Vector3();
            if (CohesionStrength != 0)
            {
                // TODO: Compute acceleration vector based on cohesion algorithm.
                // Set the length of this vector to the CohesionStrength property.
				acceleration = focusPoint - ship.Position;
				acceleration.Normalize();
				acceleration.Scale(CohesionStrength);
            }
            return acceleration;
        }

        /// <summary>
        /// Determines the acceleration vector to apply to the provided 
        /// ship based on the separation algorithm
        /// </summary>
        /// <param name="ship">the provided ship</param>
        /// <returns>the acceleration vector to apply</returns>
        private Vector3 GetSeparationAcceleration(Ship ship)
        {
            Vector3 acceleration = new Vector3();
            if (SeparationStrength != 0)
            {
                int otherShipIndex = 0;
                if (PredictCollision(ship, out otherShipIndex))
                {
                    // TODO: Compute acceleration vector based on separation algorithm.
                    // Set the length of this vector to the SeparationStrength property.
					// Determine nearest neighbor
					acceleration  = ship.Position - Ships[otherShipIndex].Position;
					acceleration.Normalize();
					acceleration.Scale(SeparationStrength);

				}
			}
            return acceleration;
        }

        #endregion

        #region INTERFACE METHODS

        /// <summary>
        /// Updates all ships currently assigned to this task force
        /// </summary>
        /// <param name="delta">seconds since last update</param>
        public virtual void Update(float delta)
        {
            if (ships.Count > 0)
            {
                // TODO: Calculate the flock focus.
				CalculateFocus();
                for (int i = 0; i < ships.Count; ++i)
                {
                    if (!(i == 0 && StickToLeader) && ships[i].Idle)
                    {
                        Vector3 accumulator = new Vector3();

                        // TODO: Use acculmulator to calculate the sum of each
                        // steering behavior acceleration vector.  Scale this
                        // sum by the product of the ship's task force speed 
                        // and time. Then add this accumulator to the ship's 
                        // velocity vector.
						accumulator += GetAlignmentAcceleration();
						//accumulator.Scale(delta);
						accumulator += GetCohesionAcceleration(ships[i]);
						//accumulator.Scale(delta);
						accumulator += GetSeparationAcceleration(ships[i]);
						//accumulator.Scale(delta);
						accumulator.Scale(ships[i].TaskForceSpeed * delta);
						//accumulator.Normalize();
						ships[i].Velocity += accumulator;						
                    }
                    ships[i].Update(delta);
                }
            }
        }

        #endregion
    }
}